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Abstract — This paper studies the problem of control strategy 
synthesis for dynamical systems with differential constraints to 
fulfill a given task specification while satisfying a set of safety 
rules. Particular attention is devoted to task specifications that 
become feasible only if a subset of the safety rules are violated. 
The proposed algorithm then computes a control law, that 
minimizes the level of unsafety for a trajectory that satisfies 
the given task specification. This problem is motivated by an 
autonomous car navigating an urban environment while follow- 
ing rules of the road such as "always travel in right lane" and 
"do not change lanes frequently". Ideas behind sampling based 
motion-planning algorithms, such as the Probabilistic Road 
Map (PRM) and Rapidly-exploring Random Tree (RRT), are 
employed to incrementally construct a finite concretization of 
the dynamics as a durational Kripke structure. In conjunction 
with this, a finite automaton that captures the safety rules is 
used in order to find an optimal trajectory that minimizes 
the violation of safety rules. It is shown that the proposed 
algorithm guarantees asymptotic optimality, i.e., almost-sure 
convergence to optimal solutions. Moreover, the algorithm is 
also demonstrated in computational experiments. 

I. Introduction 

From avoiding traffic jams in busy cities to helping the 
disabled and elderly on their daily commute, autonomous 
vehicles promise to revolutionize transportation. However, as 
they begin to transition from experimental projects like the 
DARPA Urban Challenge [1] to sharing road infrastructure 
with human drivers, we need to ensure that they obey the 
same rules that human drivers are subject to, e.g., rules of 
the road and safety rules. These rules, such as "always stay 
in the right lane" and "do not change lanes", can typically 
be expressed in formal languages such as Linear Temporal 
Logic (LTL) and deterministic /i-calculus. 

The general problem of finding optimal trajectories satis- 
fying temporal logic tasks has been studied in a number of 
recent works such as [2]-[5]. In fact, as [6] points out, one of 
the main challenges of such approaches is the abstraction of 
continuous systems into equivalent finite transition systems 
for control synthesis. Moreover, the quality of these con- 
trollers depends upon the abstracted finite transition system, 
and there is no guarantee that a controller will be found 
(if one exists), i.e., these algorithms are not complete and 
cannot be applied to, for example, dynamically changing 
environments. 

On a related note, in the robotics literature, algorithms 
based on Probabilistic Road Maps (PRMs) and Rapidly- 
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exploring Random Trees (RRTs) have been used to syn- 
thesize dynamically-feasible trajectories. Algorithms such as 
PRM* and RRT* [7] are computationally efficient counter- 
parts of these algorithms that guarantee almost sure asymp- 
totic optimality of the returned trajectories. These algorithms 
have been primarily used for motion planning, and only 
recently, they have been adapted to handle complex task 
specifications given in temporal logics [8]. 

This work focuses on the case when a task specification 
is infeasible, unless some of the rules can be temporarily 
broken. Consider, for example, an autonomous car that must 
reach its final destination while abiding by the rules of the 
road, such as avoiding collisions with obstacles and staying 
in the right lane. However, it is more important to reach the 
destination than to stay in the correct lane. In fact, the latter 
rule is temporarily violated when, for instance, there is a 
blockage on the right lane, such as a disabled car. Motivated 
by these scenarios, we would like to systematically evaluate 
control strategies that not only pick the rules to be violated 
but also quantify the level of unsafety of the trajectory and 
minimize it. In this context, our work is closest in spirit to [9] 
and [10], and it extends our previous work in [11], where 
the problem of minimum-violation control synthesis for a 
pre-defined discrete transition system was considered. 

In this paper, using ideas from sampling-based motion 
planning algorithms, we concretize a continuous -time dy- 
namical system into a finite durational Kripke structure, 
which captures the time duration required to transition be- 
tween states. We leverage automata-based model checking 
approaches to construct a weighted automaton for a given 
set of prioritized safety rules, which enables us to quantify 
the level of unsafety of finite input words as the total weight 
of an accepting run. We next propose an algorithm, MVRRT* 
(Minimum- Violation RRT*), that incrementally constructs 
the product of the Kripke structure and the weighted automa- 
ton and returns a trajectory of the dynamical system that, (i) 
minimizes the level of unsafety among all trajectories that 
satisfy the task, and (ii) minimizes a given cost function 
among all trajectories that satisfy (i). We prove that as the 
number of states of the Kripke structure goes to infinity, the 
solution converges to the optimal trajectory of the dynamical 
system that satisfies the same criteria. 

This paper is organized as follows. We introduce notation 
and preliminaries in Section II, followed by the problem 
formulation in Section III. Sections IV-V discuss details 
of the proposed algorithm. Simulations motivated by an 
autonomous car are discussed in Section VI. We conclude 
with directions for future work in Section VII. 



II. Preliminaries 

A. Durational Kripke Structures for Dynamical Systems 

Let II be a set of atomic propositions. The cardinality 
and the set of all subset of II are denoted by |II| and 2 n , 
respectively. Let X c R d and U C W 71 be compact sets. 
Consider a dynamical system given by, 

x(t) = f(x(t),u(t)), x(0)=x init (1) 

where X[ n [ t is the initial state. Trajectories of the states and 
controls are denoted by x : R>o —> X and u : R>o —> 
U respectively. In the sequel, we tacitly assume that the 
function /(•,•) is Lipschitz continuous in both its arguments 
and u is Lebesgue measurable, to guarantee existence and 
uniqueness of solutions of Equation (1). For a set of atomic 
propositions II, let C c : X — ^ 2 n be a function that maps 
each state to the atomic propositions that it satisfies. 

A timed word uj over 2 n is a finite sequence of elements 
of the set 2 n x R>q. In particular, a timed word produced 
by a trajectory x : [0, T] — » X is a finite sequence 
u(x) = (£o,do),(£ 1 ,di),...,(£ n -i,d n -i),(£ n ,d n ), where 
(i) £ = C c (x(0)) and d = 0, (ii) for all 1 < i < n - 1, 
we have £i = C c (x(t)), for all t G [U,U+i) 9 where to = 
and ti = ti-i + di, and (iii) £ n = £ n -i = C c (x(T)) and 
2™ =0 dj = T. A word produced by a trajectory x : [0, T] — >> 
X is a finite sequence = ^o?^i? • • • ?^n-i?^n> suc h 

that u> (x) = (4,d ),(^i,^i),...,(4_i,d n _i),(4,d n ) is 
the timed word produced by x. 

Definition 1 (Durational Kripke Structure) A durational 
Kripke structure is a tuple JC = (S, Si n u, II, £, A), where 

• S is a finite set of states; 

• s init £ S is an initial state; 

• 1Z C S x S is a deterministic transition relation; 

• II is a set of atomic propositions; 

• C: S ^ 2 U is a state labeling function; 

• A : 7Z — )> R>o w a function assigning a time duration 
to each transition. 

A trace of JC is a finite sequence of states r = so, si, . . . , s n 
such that so = s init and G 7£, for all 

< i < n. It produces a finite timed word u(r) = 
(C(s ), A(s ,si)), . . . , (£(s n _i, A(s n _i,s n )) and a word 
iy(r) = £(so), . . . , £(s n _i). Given a word w(r) = 

^o, • • • , £ n , let / = {io, zi, . . . , ifc} be a set of indices, such 
that z = 0, 4. = = . . . = £ ij+1 -i + £ ij+1 , for all 

< j < k — 1, and ^ = = . . . = £ n . Define 

destutter(w(r)) = i io , £ ix , ,...,£ ik . 

The following definition is used to concretize a 
continuous-time dynamical system defined in Eq. (1) into 
a durational Kripke structure. 

Definition 2 (Trace-Inclusive Kripke Structure) A dura- 
tional Kripke structure JC = (S, s^mt, II, £, A) w called 
trace -inclusive with respect to the dynamical system in 
Eq. (1) (f(7J 5 C X, (ii) (Hi) if (si,s 2 ) G ft, 

£/zere eiw^ <2 trajectory x : [0, T] — X swc/z £/z<2£ x(0) = si, 



x(T) = S2, T = A(si,S2) there exists a T' G [0,T], 
swc/z £to £ c (x(t)) = C c (si), for all t G [0,X"], and 
C c (x(t)) = C c {s 2 ), for all t G (T',T] and, (iv) C(s) = 
C c (s) far all states s G S. Roughly speaking, C c (x(t)) 
changes its value at most once between two states. 

The following lemma easily follows from the definition 
above and connects trajectories of the dynamical system with 
traces of a durational Kripke structure. 

Lemma 3 For any trace r of a trace-inclusive Kripke struc- 
ture JC, there exists a trajectory of the dynamical system, say 
x : [0,T] — >> X, such that, destutter(iu(r)) = w(x). 

B. Finite Automata 

Definition 4 (Finite Automaton) A non- deterministic finite 
automaton (NFA) is a tuple A = (Q, qinit: ^? ^ F)> where 

• Q is a finite set of states; 

• Qinit G Q is the initial state; 

• E is an input alphabet; 

• 5 C QxYixQ is a non-deterministic transition relation; 

• F C Q is a set of accepting states. 

The semantics of finite automata are defined over finite 
words produced by durational Kripke structures (see Def. 1). 
In particular, in this work, the alphabet E is chosen to be 
2 n x 2 n . A tuple r = (gi, (<ti, <t 2 ), q 2 ) G S corresponds to 
a transition labeled with (cr 1; cr 2 ) G 2 n x 2 n from a state q\ 
to state #2 • A run p of a timed automaton over a finite word 
w = £o, . . . ,£ n is a sequence qo, . . . , q n of states, such that 
qo = te. and there exists a transition (£i,£i+i), qi+i) G 
J, for all < i < n — 1. A word w is accepted if and only if 
there exists a run p = qo, . . . , q n over w, such that q n e F. 
The language of an automaton .4, denoted L(*4), is the set 
of all words accepted by A. 

An automaton A is called non-blocking if, for all q G Q, 
and ^1,^2 G E, there exists a transition (g, (£±,£2), q f ) G (5. 
For each automaton ^4 = (Q, g^t, E, 5, F) that is blocking, 
there exists a language equivalent non-blocking finite au- 
tomaton A! = (Q', ginit, ^, F), where Q' = Q U {q new }, 
and «' = *U {fe(^ 2 ),w) UGE,gGQ / }. 

C. Finite LTL 

Finite automata can capture a large class of properties 
that are exhibited by traces of a transition system. However, 
some specification languages with similar expressive power, 
such as regular expressions or variants of Linear Temporal 
Logic (LTL) interpreted over finite runs, provide a more user- 
friendly means to express these properties. Please see [12], 
[13] for a thorough exposition to regular expressions and 
LTL. We demonstrate in Section VI, how desirable properties 
of the system, e.g., the rules of the road and safety rules, can 
be conveniently captured by a slight modification of Finite 
LTL (FLTL_x) [14] without the next operator, a definition 
for which is given below. 

Definition 5 A FLTL-x formula <p over the set of atomic 
propositions II is defined inductively as follows: 



1) every pair of atomic propositions, (a, a 7 ) G II x II is a 
formula, 

2) if fa and fa are formulas, then fa V fa, ^fa, fa U fa, 
G fa, and F fa are each formulas, 

where -> (negation) and V (disjunction) are standard Boolean 
connectives, and U, G, and F are temporal operators. 

Unlike the well-known LTL (see e.g., [13]), FLTL_x 
is interpreted over finite traces, as those generated by the 
durational Kripke structure from Def. 1. Informally, (a, a 7 ) 
holds true on a trace . . . : l n if a G Iq, and a' G i\. 

The formula fa U fa states that there is a future moment 
when formula fa is true, and formula fa is true at least 
until fa is true. The formula G <j) states that formula <j) holds 
at all positions of a finite trace, and F states that holds 
at some future time instance. Just as LTL formulas can be 
algorithmically translated into automata over infinite words, 
FLTL formulas can be translated into finite automata [15]. 

D. Level of Unsafety 

Let JC = (5, Si n it,1Z,Tl : £, A) be a durational Kripke 
structure. Let A = (Q, Qinit^ 2 n , F) be the automaton for a 
safety rule with priority vo(A). In the sequel, we assume that 
the empty trace of JC and the empty word e always satisfy 
a safety rule given by any A, i.e., by convention, an empty 
trace does not violate any safety rules. 

Definition 6 (Level of unsafety for a safety rule) Let 

w — £o, . . . ,i n be a word over 2 n , define 

vanish(w, {i u . . . i k }) = £ , . . . , £ h _ u £ il+1 , ...£ n: 

i.e., the finite sequence obtained from w by erasing states 
indexed with i±, . . . , ik where {ii, . . . , i&} C {0, . . . , n}. The 
level of unsafety \(w,A) of w with respect to a safety rule 
expressed as a finite automaton A is the defined as follows. 

\(w.A) = min > w(A). 

/C{0,...,n} | v&msh(w,I)eL(A) ^ 

The level of unsafety for a trace r = so, . . . , s n +i of a 
durational Kripke structure is then defined to be 

\(r,A)= min V" Afe, s i+1 )w{A). 

iei 

Consider a sequence of non-empty sets of safety rules \I/ = 
(^i,...,^ n ) with each rule ^ G ^, for all 1 < i < 
n given in the form of a finite automaton Ai j. A priority 
function w : \l>i — ^ N assigns a priority to each rule Aij G 
^i, for all 1 < i < n. The ordered set \l> together with 
the priority function w is called a set of safety rules with 
priorities (^t,w). We now extend the definition of the level 
of unsafety for a word w and a trace r to a set of safety 
rules with priorities as follows. 

Definition 7 (Level of unsafety for a set of rules) The 

level of unsafety of a word with respect to a set of rules ^i, 
\(w^i) and the level of unsafety with respect to a set of 



rules with priorities (\I/,?Z7) are defined as, 



\(w, ^) = (\(w, i), . . . , \(w, *„)) 

Level of unsafety for a trace r of JC with respect to a set 
of rules with priorities is defined similarly. The standard 
lexicographic ordering for A(r, is used to compare the 
level of unsafety for two traces r\, T2 of JC. 

III. Problem Formulation 

For a compact set S C M d , define Si n u G S to be the 
initial state and a compact subset S goa i C S as the goal 
region. Given the dynamical system in Eq. (1), define a task 
specification $ to be, "traveling from Si n u to S goa i\ A 
trajectory of the dynamical system, x : [0, T] — » X is said 
to satisfy <l>, if x(0) = Si n u and x(T) G S goa i. Similarly, 
a trace r = sq , . . . , s n of a trace-inclusive Kripke structure 
satisfies $ if sq = s in i t and s n G c^oaZ- We tacitly assume 
that this task is feasible. 

Problem 8 Given a dynamical system as shown in Eq. (1), 
a task specification 3>, a set of safety rules with priorities 
and a continuous function c(x) that maps a trajec- 
tory x of the dynamical system to a non-negative cost, find 
a finite trajectory x* : [0, T] — >• X producing a timed word 
uj(x*) = (^o, <^o) • • • (^n> d n ) such that, 

(i) x* satisfies the task specification 

(ii) x* minimizes the level of unsafety, \(w(x') 1 among 
all trajectories x' that satisfy condition (i), 

(Hi) minimizes c(x") among all trajectories x" that 
satisfy conditions (i) and (ii). 

The solution of this problem as defined above exists if 
the task $ is feasible. In this work, we restrict ourselves 
to minimum-time cost functions, i.e., c(x) = J Idt. The 
algorithm described here however applies to a much wider 
class of functions including discounted cost as well as state 
and control based cost functions. In order to develop an 
algorithmic approach for Problem 8, we convert it to the 
following problem defined on a trace-inclusive durational 
Kripke structure. 

Problem 9 Given a durational Kripke structure JC = 
(S, Si n i t , 1Z, II, £, A) that is trace-inclusive for the dynami- 
cal system in Eq. {I), a task specification 3>, a set of safety 
rules with priorities and a cost function c(x), find 

a finite trace r* = sq, si, . . . , s n of JC and a trajectory : 
[0, T] — >• X such that they satisfy the following conditions, 

(i) x* satisfies <E>, 

(ii) there exists a set {t$,t\, . . . ,t n } such that to = 0, t n = 
T, Si = x*(ti) and A(s^, = U+i—ti V < i < n, 

(Hi) r* minimizes A(cj(r / ), among all traces r r ofJC that 
satisfy conditions (i) and (ii) and, 

(iv) minimizes the cost function c(x') among all trajec- 
tories x' that satisfy (i) and (ii). 



IV. Algorithm 

This section describes an algorithm for finding minimum- 
constraint violation trajectories for a dynamical system. We 
first construct a weighted automaton whose weights are 
chosen such that the weight of an accepting run equals 
the level of unsafety of the input word. We then propose 
an algorithm, based on RRT*, to incrementally construct 
the product of the durational Kripke structure and weighted 
automata representing safety rules. Roughly, the shortest 
path in the product uniquely maps to a trace of the Kripke 
structure that minimizes the level of unsafety. Let us note 
that the algorithm returns a trajectory that satisfies all rules 
and minimizes the cost function if it is possible to do so. 

A. Weighted product automaton 

First, we propose a method to augment each automaton 
Aij G with new transitions and weights, such that the 
resulting weighted automaton also accepts all words w that 
do not satisfy the rule Aij', however, the weights are picked 
such that the weight of this accepting run over w determines 
the level of unsafety of w with respect to Aij (see Def. 10). 
Second, we combine all the weighted automata into a single 
weighted automaton A<&, using its weights to capture the 
level of unsafety of all words with respect to the given set 
of safety rules with priorities (\l>, w) (see Def. 12). Third, we 
build a weighted product of the durational Kripke structure 
K and the automaton A& (see Def. 14) with the weights 
corresponding to the level of unsafety of traces of /C. 

We now proceed to describe each of these steps in detail 
and summarize the purpose of each construction in a lemma 
(see Def. 10-14 and Lem. 11-15). The material presented in 
this section is a slight modification of our earlier algorithm 
for finding a trace of a weighted transition system that 
minimizes the level of unsafety [11]. For the sake of brevity, 
proofs of these lemmas are omitted and can be found in [1 1]. 

Definition 10 (Automaton A^ id ) For a non-blocking finite 

automaton Aij = (Qij, Qinit,i,j, 2 n , 5ij, Fij), a weighted 
finite automaton 

Aij — (Qij, Qinit,i,ji ^ , Sij , Fij ,Wij) 

is such that, = Q iJ} q initij = q in it,ij, = S itj U 
(<r, a'), q) | q G Q and (a, a') G 2 n }, Fij = Fij and, 

w(r) = (° , if f Te5 i\, 

v [ w(Aij) if re 6ij \ dij. 

Lemma 11 If a word w over 2 n is accepted by Aij, then, 
the weight of the shortest accepting run of Aij over w is 
equal to the level of unsafety \(w,ijjij). 

A single weighted automaton is created by combining 
all automata Aij, where Aij G G This captures the 
level of unsafety with respect to the whole set of safety rules 
with priorities {*&,vj) through its weight function. 



Definition 12 (Automaton ^4^) The weighted automaton 

M = (Q,q mit ,2 u ,C,S,F,W) 
is defined as follows: 

• Q = Ql,l • • • X . . . Ql,mi • • • X . . . Qn,l • • • X . . . Q n ,m n y 

• Qinit (^im£,l,l' ■ • • ) Qinit,n,m n ^) > 

. (p,(a,(T'),p') eS if 

- P = (gi 1 lr-- > ?n,m n ), P* = (<?1,1 , ••■ ^n,m n )> and 

- (qijifarfiQij) € S id , for alii G {l,...,n},j G 
{1, . . .m*}. 

Then also (cr, cr'),^/)) = . . . , x n ), where 

x _i = E^i W^, j fe, j ,(c^,c^ / ),gy; 
• F = {(<? M , . . . ,q n ,m n ) I qij e Fij, for all 
i G {l,...,n},j G {l,...mj} 

Lemma 13 Any word w over 2 n is accepted by A<& and the 
weight of the shortest accepting run of A\& over w is equal 
to the level of unsafety \(w,ty). 

Definition 14 (Weighted product automaton V) We build 
the weighted product automaton, 

V = JC (g) A* = (Qv,qinit,v,5 v ,F<p,Wv) 

of the Kripke structure JC = (5, Si n a, 71, II, C, A) and the 
augmented automaton A\& = (Q, q init , 2 n , 5, F, W) as, 

• Q-p = S x Q is a set of states; 

• qinit,v = (siniuqinit) is the initial state; 

• ^ C Q v x Q-p is a non- deterministic transition 
relation, where ((s,q), (s f ,q f )) G 5-p if(s,s f ) G 1Z, 
and there exists a transition (q, (C(s), C(s f )), q') G S. 
Then also, 

W v ((s, q), (s\ q')) = ( Xl • A(s, s f ), . . . , x n ■ A(s, s')), 

where (xi, . . . , x n ) = W(q, C(s), C(s'), q') and, 

• Fp = S x F is a set of accepting states. 

The alphabet of the product automaton is trivial, and 
therefore omitted. A product automaton is in fact, a finite 
automaton extended with weights. A run of a product au- 
tomaton is a sequence p = po,...,p n , such that po = 
qinit,v> an d (Pi,Pi + 1) G 5-p, for all < i < n and it is 
accepting if p n G F. The weight of a run Wp(p) is the tuple 
obtained by component-wise sum of the weights associated 
with the transitions executed along the run. The shortest run 
over w is then a run p minimizing the weight Wp(p) in the 
lexicographical ordering. 

Lemma 15 The shortest accepting run (in the lexicograph- 
ical ordering with respect to W-p ), po . . . p n of V from the 
state po = qinit,v to a state p n G Fp projects onto a trace 
r = a(po • • - Pn) of JC that minimizes the level of unsafety. 

B. Incremental Weighted Product Automaton 

In this section, we incrementally construct the weighted 
product automaton (see Def. 14) and maintain the trace that 
minimizes the level of unsafety for a set of safety rules \l>. A 
few preliminary procedures of the algorithm are as follows. 



1) Sampling: The Sample procedure samples an indepen- 
dent, identically distributed state s from a uniform distribu- 
tion supported over the bounded set S. 

2) Nearest neighbors: The Near procedure returns the set, 

Snear(s) = {s'\ \\s' - s\\ 2 < 7 (log u/fl) 1/d ] s' G S~\ 

where n = \S\ and 7 is the constant in Thm. 16. 

3) Steering: Given two states s, s', the Steer(s',s) pro- 
cedure computes the pair (x, T) where x : [0, T] — » X is a 
trajectory such that, (i) x(0) = s', (ii) x(T) = s and, (iii) 
x minimizes the cost function c(x) = T. If a trajectory x is 
found, return true, else return false. 

4) Connecting: For a state s' G S near , if Steer(V,s) 
returns true, for all nodes z' = (s f , g') G Q-p, if (z f , (s, g)) 
is such that it can be in 5-p (see Def. 14), the procedure 
Connect (V , s) adds the state z = (s,q) to the set Q^, adds 
(z',z) to J-p and calculates W-p(z',z). If s G S goa i and 
<7 G F, it adds (s, q) to F<p. 

5) Updating costs: The procedure Update (s) updates the 
level of unsafety J a (z) and the cost Jt(s) from the root for 
a node z = (5, g) as shown in Alg. 2 using the sets, 

S 3 teer (s) = {«' | s' G SnearO 5 ); Steer(V, s) returns true}, 

Z steer (s) = {(s',q') \ s' G S ate er(s)] 5 ', tf') G Qp}. 

6) Rewiring: In order to ensure asymptotic optimality, the 
Rewire procedure recalculates the best parent Par(s') for all 
states s f G S near (s) as shown in Alg. 3. The complexity of 
this procedure can be reduced by noting that s' only needs 
to check if the new sample can be its parent by comparing 
costs JajJt, otherwise its parent remains the same. 

Finally, Alg. 1 creates the weighted product automaton as 
defined in Def. 14 incrementally. It also maintains the best 
state = (s*,q*) G F v . The trace r* = s , si, . . . , s n of 
the Kripke structure JC that minimizes the level of unsafety 
and is a solution to Problem 9 can then be obtained from 
z* by following Par (5*). Since JC is trace-inclusive, the 
continuous-time trajectory x* can be obtained by concatenat- 
ing smaller trajectories. Let [x^Ti) be the trajectory returned 
by Steer(^, s^+i) for all states S{ G r*. The concatenated 
trajectory : [0, T] X is such that T = and 

X n (t + E/c=0 = X * W f0r a11 * < n ' 

V. Analysis 

In this section, we analyze the convergence properties of 
Algorithm 1. In particular, we prove that the continuous- 
time trajectory x n given by the algorithm after n iterations 
converges to the solution of Problem 8 as the number of 
states in the durational Kripke structure JC n goes to infinity, 
with probability one. An analysis of the computational 
complexity of the algorithm is also carried out here. Due 
to lack of space, we only sketch the proofs. 

A. Asymptotic Optimality 

Theorem 16 The probability that Algorithm 1 returns a 
durational Kripke structure JC n and a trajectory of the 
dynamical system x n , that converges to the solution of 



Algorithm 1: Create Product 


1 Input 


: n, <S, 


2 Output : Vn\ 


3 V <- 

4 i C 


0; Q v <- qv **; Ja(s ini t) <- 0; Jt(s ini t) <- 0; 


5 for i 


; 

< n do 




/ / Sample new state 


6 


s 


<(— Sample; 




/ / Connect neighbors 


7 


for s' G Near(s) do 


8 




if Steer (s x , s) then 


9 




|_ Connect (s x , s); 




/ / Find best parent 


10 


Par, J a ,Jt <- Update (s); 




/ / Rewire neighbors 


11 


V,J a ,Jt ^— Rewire (s); 


12 Vn <- 




13 return V n 



Algorithm 2: Updat e(s,V) 


1 for z = (s, q) G Qp do 


2 


Jo (2) «- min W-p(z , z) + J a (> ); 


3 


Z* <(— arg min W-p(V , z) + J a (z)\ 

z f eZ steer 


4 


JAs) 4- min c(s ; , s) + Jt(s')\ 

z'eZ* 


5 


Par(z) ^— arg min c(V, s) + Jt(s'); 

z'ez* 


6 return Par, J a , Jt 



Algorithm 3: Rewire(s,V) 


1 for s' G 5' s t eer (s) do 


2 


if Steer(s, s x ) then 


3 


|_ Connect(s, s); 


4 


J a , Jt ^— Update {s')\ 


5 return V 



Problem 8 in the bounded variation norm sense, approaches 
one as the number of states in K n tends to infinity, i.e., 

P({lim \\x n -x*\\ BV = ti\) =1 



Proof: (Sketch) The prof primarily follows from the 
asymptotic optimality of the RRT* algorithm (see Theorem 
34 in [7]). Let x* : [0, T] H> X be the solution of Problem 8 
that satisfies the task $ and minimizes the level of unsafety. 
For a large enough n, define a finite sequence of overlapping 
balls B n = {J? n5 i, . . . , J?n,™} around the optimal trajectory 
x*. The radius of these balls is set to be some fraction of 
7 (log njn) x l d such that any point in s G i? n ,m can connect 
to any other point s' G J? n?m+ i using the Steer(s, s') 
function. It can then be shown that each ball in B n contains 
at least one state of JC n with probability one. In such a 
case, there exists a trace r n = sq, si, . . . , s n of JC n such 
that every state lies in some ball J? n , m . Also, for a large 
enough n, the level of unsafety of r n , A(r n , \l>) is equal to 
the level of unsafety of the word generated by the trajectory 
x*, X(cj(x*),^f), i.e., MVRRT* returns the trace with the 
minimum level of unsafety among all traces of the Kripke 



structure K satisfying the task 0. Finally, it can be shown 
that the output of the algorithm, trajectory x n converges to 
the optimal solution x* almost surely as n —> oo. 

In this proof, 7 > 2 (2 + \) 1/d (^) 1A \ where /x(5) is 
the Lebesgue measure of the set <S, is the volume of the 
unit ball in S which is of dimensionality d. ■ 

The following lemma is an immediate consequence of 
Theorem 16 and the continuity of the cost function c(x). 

Lemma 17 The cost of the solution converges to the optimal 
cost, c* = c(x*), as the number of samples approaches 
infinity, almost surely, i.e, 

P ({ lim c(x n ) = c*}) = 1. 

B. Computational Complexity 

Let us now comment on the computational complexity of 
MVRRT*. 

Lemma 18 The expected computational complexity of 
MVRRT* is 0(m 2 log n) per iteration where n is the number 
of states in the durational Kripke structure K and m is the 
number of states of the automaton (see Def 12). 

This follows from an analysis of Algorithm 1. Note that 
there are an expected O(logn) samples in a ball of ra- 
dius 7(logn/n) 1 / d . The procedure Steer is called on an 
expected O(logn) samples while because the automaton 
is non-deterministic, the procedure Connect adds at 
most m 2 new states in the product automaton per sample. 
The procedure Update requires at most G(m 2 log n) time 
call. The Rewire procedure simply updates the parents of 
the O(logn) neighboring samples which take G(m 2 log n) 
time. In total, the computational complexity of MVRRT* is 
G(m 2 logn) per iteration. 

VI. Computational Experiments 

In this section, we consider an autonomous car modeled as 
a Dubin's vehicle in a road-like environment along with road- 
safety rules and we evaluate the performance of MVRRT* 
in a number of different situations. 

A. Experimental Setup 

Consider a Dubin's car, i.e., a curvature-constrained vehi- 
cle with dynamics given by the differential equations, 

x(t) = v cos 0(t) 

y(t) = vsm6(t) (2) 
0(t) = u(t) 

The state of the system is the vector [x, y, 0] T , and the 
input is u(t), where \u(t)\ < 1 for all t > 0. The vehicle is 
assumed to travel at a constant speed v. As shown in [16], 
time-optimal trajectories for this system in an obstacle-free 
environment can be easily calculated. 

Each state of the Dubin's vehicle is labeled with the set 
of atomic propositions that hold true in that state. Namely, 




Fig. 1: Partitions of the working domain S. The transition from si to 
S2 is labeled with, for example, {(rZ, 11), (rZ, -^dir), (rZ, dotted)}. 

the atomic propositions capture, (i) whether the vehicle is 
present in the right lane of the road, the left lane of the 
road, or the sidewalk, (ii) whether the vehicle is heading in 
the correct direction or not, and ( Hi) whether the center line 
is dotted or solid. 

We therefore partition the working domain S into compact 
non-empty subsets <S 6s, S sw , S r i, and Su, where S Q bs is 
the union of regions occupied with obstacles, S sw represents 
the sidewalk and S r i, Su are the right and the left lanes, 
respectively. In the presence of no obstacles, S b s is empty. 
These partitions are illustrated in Fig. 1. Based on this 
partitioning and the current state of the Dubin's vehicle, we 
define the set of atomic propositions as, 

II = {sw, rZ, ZZ, dir, dotted, solid}. 

For a state s G S in the trace-inclusive durational Kripke 
structure JC, the proposition sw is true if s G S sw . Similarly, 
the atomic proposition rZ is true if s G S r i, and ZZ is true if 
s G Su. The propositions rZ and ZZ cannot both be true at the 
same time. The proposition dir models whether the heading 
of the car is in the correct direction or not, i.e., dir is true 
if the state s is such that it is heading forwards and rZ is 
true. Note that dir can be calculated using the state s and 
the geometry of the road. Furthermore, one of the atomic 
propositions dotted and solid is always true, depending on 
the lane on which the vehicle's state is located. Note that 
obstacles are not considered while constructing II since we 
do not desire a trajectory that goes over an obstacle in any 
case. Hence, the Steer procedure in Section IV returns false 
if any state along the trajectory lies in 5 & s . This change 
does not affect the correctness and the overall complexity of 
MVRRT*. 

B. Safety Rules 

Given a task $ such as finding a trajectory from s init to 
the goal region S goa u we require the vehicle to follow the 
following rules: (i) do not travel on the sidewalk (sidewalk 
rule), (ii) do not cross the solid center line (hard lane chang- 
ing), (Hi. a) always travel in the correct direction (direction 
rule), and (iii.b) do not cross the dotted center line (soft lane 
changing). 

We describe the rules with the following FLTL formulas 
and corresponding finite automata in Fig. 2. Note that we use 
2-tuples of atomic propositions from II as the alphabet for 



both formulas and the automata, so that we can specify not 
only properties of individual states, but also of transitions. 
The first component of the tuple captures the atomic propo- 
sition true in the starting state of the transition; the second 
component represents the atomic proposition of the ending 
state. 

(i) Sidewalk: Do not take a transition that ends at a state 
labeled sw. 

^1,1 = G /\ -i(*,sw) 

*G2 n 

(ii) Hard lane change: Do not take any transition that 
changes lanes and crosses a solid center line. 

^2,1 = G(^((rl, solid) A (rl,ll)) V ((11, solid) A («,rZ))) 

(Hi. a) Direction: Do not take any transition that leads to 
motion in the wrong direction. 

^3,1 = G V (*, dzr) 

*G2 n 

lane change: Do not take any transition that 
changes lanes and crosses a dotted center line. 

^3,2 = G ((rZ, dotted) A (rZ, ZZ)) V ((ZZ, dotted) A (ZZ, rZ))) 

The finite automata for rules (i)-(iii.b) are all of the same 
form (see Fig. 2). 




Fig. 2: Direction rule. For brevity of presentation, there is only one 
transition depicted representing in fact all the transitions, where ( i) 
£,£' C 2 n , £ is arbitrary and dir £ £', (ii) £,£' C 2 n , such that 
rl £ I and dotted, 11 £ or 11 £ £ and dotted, rl £ (in) 
C 2 n , such that rl £ £ and solid, 11 € f, or 11 £ £ and 
so/id, rZ G ^, and (7v) £, f C 2 n , £ is arbitrary and f 

While it is quite natural to disobey the direction and the 
soft lane change rules, the solid lane should not be crossed 
and especially, the sidewalk should be entered only if there 
is no other way to reach the target region. This gives three 
different priority classes 

(*l,*2,*3),^) = (({^l,l},{^2,l}, {^3,1,^3,2}),^), 

where ^(/0i 5 i) ^7(^2, 1) ^(^3,1) = 1 and m (1/13 2) 
1 0. Note that costs for ip 2 ,i and ^2 are incurred only once 
per crossing and do not depend upon the duration of the 
transition. Within the third class, we put higher priority on 
the soft lane change rule to avoid frequent lane switching, 
for instance in case two obstacles are very closed to each 
other and it is not advantageous to come back to the right 
lane for a short period of time, e.g., see Fig. 4. 

C Examples 

MVRRT* was implemented in C++ on a 2.2GHz processor 
with 4GB of RAM for the experiments in this section. 
We demonstrate the performance of MVRRT* in a number 
of different scenarios in the same environment to be able 
to quantitatively compare the performance. In Fig. 4, the 




Fig. 3: MVRRT* tree after 40 sec. on an example without any 
safety rules. The shortest trajectory shown in red to the goal region 
avoids obstacles but uses the sidewalk. 



Dubin's vehicle starts from the lower right hand corner while 
the goal region marked in green is located in the lower left 
hand corner. Light grey denotes the right and left lanes, S r i 
and Su. A sidewalk S sw is depicted in dark grey. The dotted 
center line is denoted as a thin yellow line while solid center 
lines are marked using double lines. Stationary obstacles in 
this environment are shown in red. 

a) Case 1: First, we consider a scenario without any 
safety or road rules. The MVRRT* algorithm then simply 
aims to find the shortest obstacle-free trajectory from the 
initial state to the goal region. Note, that in this case, 
MVRRT* performs the same steps as the RRT* algorithm. 
The solution computed after 40 seconds has a cost of 88.3 
and is illustrated in Fig. 3 together with the adjoining tree. 

b) Case 2: Next, we introduce the sidewalk rule ipi^ 
and the direction rule ^3^. Without any penalty on frequent 
lane changing, the algorithm gives a trajectory that goes 
back into the right lane after passing the first obstacle. It 
has to cross the center line again in order to pass the second 
obstacle and reach the goal region. Fig 4a depicts the solution 
obtained after 60 seconds, this has a cost of 122.3 along with 
a level of unsafety of 46.4 for breaking ^3^. 

Upon introducing the rule ^3,2, the vehicle does not go 
back into the right lane after passing the first obstacle. 
Figure 4b shows this solution with a level of unsafety of 
84.1 for breaking both -0 3jl and ^2 whereas the level of 
unsafety in this case for the trajectory in Fig. 4a is 87.4. 

In Fig. 4c, we only have the sidewalk and direction rules. 
Since the road is completely blocked, MVRRT* is forced to 
break the sidewalk rule resulting in a level of unsafety of 
13.9 47.3 for rules ^i 5 i and ^3^ respectively. 

c) Case 3: Fig 5a shows a run for the sidewalk, 
direction and the soft lane changing rule after 60 sees, of 
execution with a level of unsafety of (0, 0, 28.3). As shown 
in Fig. 5b, if instead executed for 120 sees., the algorithm 
converges to a solution which has a much higher cost (215.8) 
but a significantly lower level of unsafety (0, 0, 1.6) because 
it only breaks the direction rule slightly when it turns into the 
lane. This thus demonstrates the incrementality and anytime 
nature of the algorithm. 

d) Case 4: In our last example, we introduce hard and 
soft lane changing rules along with sidewalk and direction 
rules. After 15 sees., MVRRT* returns the solution shown 



(c) 

Fig. 4: Fig. 4a shows the solution after 60 sees, for the sidewalk 
and direction rules. Upon introducing the soft lane changing rule in 
Fig. 4b, the vehicle does not return to the right lane after passing 
the first obstacle. Since the road is completely blocked in Fig. 4c, 
breaking the sidewalk rule is unavoidable 

in Fig. 5c, which breaks the hard lane changing rule twice, 
thereby incuring a level of unsafety of (0,2,48.1) for the 
three rules. On the other hand, after about 300 sees., the 
solution converges to the trajectory shown in Fig. 5d which 
breaks the hard lane changing rule only once, this has a level 
of unsafety of (0,1,25.17). 

VII. Conclusions 

Motivated by problems in autonomous vehicle navigation, 
this paper considered the problem of synthesizing minimum- 
violation control strategies for continuous dynamical systems 
that are subject to a set of safety rules and satisfy a given 
task specification. In particular, we focus on the case when 
the task is infeasible given the set of rules. We leverage 
ideas from sampling-based motion-planning algorithms and 
automata-based model checking approaches to propose an 
incremental algorithm to generate a trajectory of the dynam- 
ical system that picks which safety rules to violate while at 
the same time, it also minimizes the level of unsafety. 

Future directions for work include investigation of timed- 
LTL formulae, modeling rules such as yielding at cross-roads 
as well as an implementation of the proposed algorithm on 
an experimental autonomous vehicle. 
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